#include "Acomb_func.h"

bool Find_ximin(float *xi_min,float input1,float obj1,float input2,float obj2, 
		float *output1,float *output2)
{
	float xi=sqrt(pow(input1-obj1,2)+pow(input2-obj2,2));

	if(xi<*xi_min){
		*xi_min = xi;
		*output1 = input1;
		*output2 = input2;
		return(true);
	}
	return(false);

}


bool Combine2_direct(TLorentzVector mom1, TLorentzVector mom2,float cut_mlow, float cut_mup, float cut_ptlow, float cut_ptup, float *mass_re){

	TLorentzVector addm(0,0,0,0);

	addm = mom1+mom2;
	float mass_comb=addm.M();
	float pt_comb  =addm.Pt();

	// mass window cut
	if( mass_comb< cut_mlow  || mass_comb > cut_mup ){
		return(false); 
	}
	if( pt_comb  < cut_ptlow || pt_comb   > cut_ptup ){
		return(false); 
	}
	*mass_re=mass_comb;
	return(true);
}


bool Combine2SameFS_TLorentzvector(CDraw &para, TH1 *comb, std::vector<TLorentzVector> &in,float obj_mass, float cut_mlow, float cut_mup, float cut_ptlow, float cut_ptup, TLorentzVector &choosed){


	int num = in.size();
	int loop1,loop2;
	int count1 = -10,count2 = -10;
	float massclose=100000;
	float re_mass=100000.0;
	TLorentzVector mom1,mom2;
	bool judge;

	for(loop1=0;loop1<num-1;loop1++){
		for(loop2=loop1+1;loop2<num;loop2++){

			mom1 = in[loop1];
			mom2 = in[loop2];
			judge=Combine2_direct(mom1, mom2,cut_mlow,cut_mup, cut_ptlow, cut_ptup, &re_mass);
			if(judge && std::abs(re_mass-obj_mass)<std::abs(massclose-obj_mass)){
				count1 = loop1;
				count2 = loop2;
				massclose = re_mass;
			}
		}
	} 
	if(count1!=-10&& count2!=-10){
		choosed=in[count1]+in[count2];
	}
	else{return(false);}

	return(true);
}


bool Combine2DifferentFS_TLorentzvector(CDraw &para, TH1 *comb, std::vector<TLorentzVector> in1,std::vector<TLorentzVector> &in2,float obj_mass, float cut_mlow, float cut_mup, float cut_ptlow, float cut_ptup, TLorentzVector &choosed){

	int num1 = in1.size(), num2=in2.size();
	int loop1,loop2;
	int count1 = -10,count2 = -10;
	float massclose=100000;
	float re_mass=100000.0;
	TLorentzVector mom1,mom2;
	bool judge;

	for(loop1=0;loop1<num1;loop1++){
		for(loop2=0;loop2<num2;loop2++){

			mom1 = in1[loop1];
			mom2 = in2[loop2];
			judge=Combine2_direct(mom1, mom2,cut_mlow,cut_mup, cut_ptlow, cut_ptup, &re_mass);
			if(judge && std::abs(re_mass-obj_mass)<std::abs(massclose-obj_mass)){
				count1 = loop1;
				count2 = loop2;
				massclose = re_mass;
			}
		}
	} 
	if(count1!=-10&& count2!=-10){
		choosed=in1[count1]+in2[count2];
	}
	else{return(false);}

	AFill_Plot(para,comb,massclose);
	return(true);
}





bool Combine4SameFS_TLorentzvector(CDraw &para, TH1 *comb1, TH1 *comb2, std::vector<TLorentzVector> &in,
		float obj_mass1, float obj_mass2,
		float cut_mlow1, float cut_mup1, float cut_ptlow1, float cut_ptup1,
		float cut_mlow2, float cut_mup2, float cut_ptlow2, float cut_ptup2,
		std::vector<TLorentzVector> &choosed){

	para.status.Set_Current_Position("begin Combine4SameFS_TLorentzvector");

	if(in.size()<4) {
		return false;
	}

//	int count[4] = {-10,-10,-10,-10};
	float re_mass1=100000.0, re_mass2=100000.0;
	TLorentzVector mom1,mom2,mom3,mom4;

	float xi_min=std::pow((100000-obj_mass1),2)+std::pow((100000-obj_mass2),2);
	std::vector<int> count;
	std::vector<int> arr; 
	for(int i=0;i<in.size();i++){
		arr.push_back(i);
		count.push_back(-10);
	}
	do{
		mom1 = in[arr[0]];
		mom2 = in[arr[1]];
		mom3 = in[arr[2]];
		mom4 = in[arr[3]];
		bool judge1=Combine2_direct(mom1, mom2,cut_mlow1,cut_mup1, cut_ptlow1, cut_ptup1, &re_mass1);
		bool judge2=Combine2_direct(mom3, mom4,cut_mlow2,cut_mup2, cut_ptlow2, cut_ptup2, &re_mass2);
		if(judge1&&judge2){
			float xi=std::pow((re_mass1-obj_mass1),2)+std::pow((re_mass2-obj_mass2),2);
			if(xi<xi_min){
				xi_min=xi;
				memcpy(&count[0], &arr[0], sizeof(arr) );
			}
		} 
	}while (std::next_permutation(arr.begin(),arr.end() ));

	if(count[0]!=-10){
		choosed[0]=(in[count[0]]+in[count[1]]);
		choosed[1]=(in[count[2]]+in[count[3]]);
	}
	else{return(false);}

	AFill_Plot(para,comb1,choosed[0].M());
	AFill_Plot(para,comb2,choosed[1].M());
	return(true);
}

bool CombineMoreWith2DifferentFS_TLorentzvector(CDraw &para, TH1 *comb, std::vector<TLorentzVector> &in1,std::vector<TLorentzVector> &in2, float obj_mass, std::vector<TLorentzVector> &Vcombine){
	int num1 = in1.size(), num2=in2.size();
	TLorentzVector mom1,mom2,combine;
	bool judge;

	para.debug.Message(5,23,"in the CombineMoreWith2DifferentFS_TLorentzvector: begin");
	para.status.Set_Current_Position("begin CombineMoreWith2DifferentFS_TLorentzvector");
	if(in1.size()<2||in2.size()<2) {
		para.debug.Message(5,23,"in the CombineMoreWith2DifferentFS_TLorentzvector: input vector too small, they should larger than 2:",in1.size(),in2.size());
		return false;
	}

	para.debug.Message(5,23,"in the CombineMoreWith2DifferentFS_TLorentzvector: begin combine");
	TLorentzVector V1, V2;
	TLorentzVector V3, V4;
	V1=in1[0]+in2[0];
	V2=in1[1]+in2[1];

	V3=in1[0]+in2[1];
	V4=in1[1]+in2[0];

    float xi_min1=10000.0;
    float xi_min2=10000.0;
	float Fmass1, Fmass2;
	//ShowMessage(2,"in more with 2", xi_min1,xi_min2,obj_mass,V1.M(),V2.M(),V3.M(),V4.M());
	bool judge_xi1=Find_ximin(&xi_min1,V1.M(),obj_mass,V2.M(),obj_mass,&Fmass1, &Fmass2);
	bool judge_xi2=Find_ximin(&xi_min2,V3.M(),obj_mass,V4.M(),obj_mass,&Fmass1, &Fmass2);
	if(xi_min1<xi_min2){
		Vcombine[0]=V1;
		Vcombine[1]=V2;
	}
	else{
		Vcombine[0]=V3;
		Vcombine[1]=V4;
	}
	//ShowMessage(2,"end more with 2", Vcombine[0],Vcombine[1]);
	//AFill_Plot(para,comb,Vcombine[0].M());
	//AFill_Plot(para,comb,Vcombine[1].M());
	para.debug.Message(5,23,"in the CombineMoreWith2DifferentFS_TLorentzvector: end, all combined momentum",Vcombine);

	return(true);

}

bool Combine2DifferentAngles_TLorentzvector(CDraw &para, TH1 *comb, std::vector<TLorentzVector> &in1, std::vector<TLorentzVector> &in2, std::vector<float> &Values){

	TLorentzVector mom1,mom2,mom3,mom4;
	int num1 = in1.size(), num2=in2.size();
	for(int loop1=0;loop1<num1;loop1++){
		for(int loop2=0;loop2<num2;loop2++){

			mom1 = in1[loop1];
			mom2 = in2[loop2];
			float value = std::cos(in1[loop1].Angle(in2[loop2].Vect())) ; 
			Values.push_back(value);
		}
	}

	return(true);
}

bool CombineSameAngles_TLorentzvector(CDraw &para, TH1 *comb, std::vector<TLorentzVector> &in, std::vector<float> &Values){

	TLorentzVector mom1,mom2,mom3,mom4;
	std::vector<int> arr; 
	for(int i=0;i<in.size();i++){
		arr.push_back(i);
	}
	do{
		mom1 = in[arr[0]];
		mom2 = in[arr[1]];
		float value = std::cos(mom1.Angle(mom2.Vect())) ; 
		Values.push_back(value);
	}while (std::next_permutation(arr.begin(),arr.end() ));
	return(true);
}
